//
// lunohod
// AK 201404
//

#include "lunohod.h"

lunohod lun;
int led = 13;          // scan indicator
int scanCoef = 4;
int scanPeriod = 1000/scanCoef;  // scan period
int prevAction = 3;
int counAction = 0;
int counSpeed = 0;


long MillisPrev = 0;

long fronDist = 0;
long rearDist = 0;

int  fdStat = 0;
int  rdStat = 0;

// const int aForward  = 0;
// const int aBackward = 1;
// const int aRotate   = 2;
// const int aStop     = 3;
// const int aSkip     = 4;

const int goLogic[2][2][5] = { { {aStop,  aStop,      aStop,  aStop,     aStop    },  {aForward,  aStop,  aStop,    aForward, aForward} },
                               { {aStop,  aBackward,  aStop,  aBackward, aBackward},  {aStop,     aStop,  aRotate,  aRotate,  aRotate } } };

const int speedList[5][3] = { {sMIN, sMED, sMAX},    // 0 forward
                              {sMIN, sMED, sMAX},    // 1 backward
                              {sMAX, sMED, sMED},    // 2 rotate
                              {sMAX, sMAX, sMAX},    // 3 stop
                              {sMAX, sMAX, sMAX} };  // 4 skip
                             
void setup()
{
 pinMode(led, OUTPUT);
 lun.set_motor_1(2, 4, 3);  // in1 in2 en
 lun.set_motor_2(7, 8, 5);  
 lun.set_ultrasonic_1(A3, A2, 11); // trig echo led 
 lun.set_ultrasonic_2(A1, A0, 12); 
 lun.maxdist = 50; // set max in cm
// Serial.begin(9600);
}

void loop()
{
 unsigned long currentMillis = millis();
 if(currentMillis - MillisPrev > scanPeriod){
   MillisPrev = currentMillis;
   digitalWrite(led, HIGH);   
//   if(prevAction == aStop){ // x x stop
//     prevAction = aSkip;
//     lun.stop();
//   }else{
     fronDist = lun.distance(1);
     rearDist = lun.distance(2);
     
//     Serial.print(fronDist);
//     Serial.print(" ");
//     Serial.print(rearDist);
//     Serial.print(" - ");
     
     if(fronDist < lun.maxdist) fdStat = 1;
     else                       fdStat = 0;
     
     if(rearDist < lun.maxdist) rdStat = 1;
     else                       rdStat = 0;
     
     if(prevAction == goLogic[fdStat][rdStat][prevAction]) counAction++;
     else                                                  counAction=0;
     
     counSpeed = counAction;
     if(counSpeed > 2) counSpeed=2;

     prevAction = goLogic[fdStat][rdStat][prevAction];
 //    if(prevAction < 0 || prevAction > 3) prevAction = 3;
     
     lun.goLun(goLogic[fdStat][rdStat][prevAction], speedList[goLogic[fdStat][rdStat][prevAction]][counSpeed]);
     
     if(goLogic[fdStat][rdStat][prevAction] == aStop) delay(200);
     
     if(goLogic[fdStat][rdStat][prevAction] == aStop && counAction > 40/scanCoef) {
//       lun.goLun(goLogic[1][1][4], speedList[goLogic[1][1][4]][counAction]);
//       delay(2000/scanCoef);
       lun.maxdist=lun.maxdist+15;
     }else{
       lun.maxdist = 50;
     }
//     Serial.print(lun.maxdist);
//     Serial.print(" "); 
//     Serial.print(counAction);
//     Serial.print(fdStat);
//     Serial.print(" ");
//     Serial.print(rdStat);
//     Serial.print(" ");
//     Serial.print(prevAction);
//     Serial.print(" - ");
//     Serial.println(goLogic[fdStat][rdStat][prevAction]);
     
//   }
   digitalWrite(led, LOW);
 }  
}

